#include "DistanceDrive.h"

DistanceDrive::DistanceDrive(double Distance) {
	// Use requires() here to declare subsystem dependencies
	// eg. requires(chassis);
	Requires(drivetrain);
	SetPoint = Distance;
	latch = true;
}

// Called just before this Command runs the first time
void DistanceDrive::Initialize() {
}

// Called repeatedly when this Command is scheduled to run
void DistanceDrive::Execute() {
	if (latch) {
		CommandBase::drivetrain->DriveDistance(SetPoint, SetPoint);
	}
	latch = false;
	drivetrain->CheckDistanceDrive();
}

// Make this return true when this Command no longer needs to run execute()
bool DistanceDrive::IsFinished() {
	return (!drivetrain->distanceDriving && !latch);
}
// Called once after isFinished returns true
void DistanceDrive::End() {
	latch = true;

}

// Called when another command which requires one or more of the same
// subsystems is scheduled to run
void DistanceDrive::Interrupted() {
	latch = true;

}
